#ifndef     __I2C_H
#define     __I2C_H

#include <stdint.h>

namespace pub_driver {

template <uint8_t ADDR, typename SCL, typename SDA>
class I2cInterface {

public:
    static constexpr uint8_t WRITE_ADDR = ADDR << 1;
    static constexpr uint8_t READ_ADDR = (ADDR << 1) + 1;

    static bool read(uint8_t reg, uint16_t &val) {
        bool flag = false;
        uint8_t h, l;

        start();

        write_byte(WRITE_ADDR);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

        write_byte(reg);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

        start();

        write_byte(READ_ADDR);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

        h = recv_byte();
        write_ack();

        l = recv_byte();
        write_no_ack();
        
        val = (h << 8) + l;
    finish:
        stop();
        return flag;
    }

    static bool write(uint8_t reg, uint16_t val) {
        bool flag = false;

        start();

        write_byte(WRITE_ADDR);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

        write_byte(reg);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

        write_byte(val >> 8);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

        write_byte(val);
        flag = wait_ack();
        if (!flag) {
            goto finish;
        }

    finish:
        stop();
        return flag;
    }

    static void start() {
        SCL::set();
        SDA::set();
        delay();
        SDA::clr();
        delay();
        SCL::clr();
        delay();
    }

    static void stop() {
        SCL::set();
        SDA::clr();
        delay();
        SDA::set();
        delay();
    }

    static void write_byte(uint8_t val) {
        for (uint8_t i = 0; i < 8; i ++) {
            if (val & 0x80) {
                SDA::set();
            } else {
                SDA::clr();
            }
            val <<= 1;
            delay();
            SCL::set();
            delay();
            SCL::clr();
            delay();
        }
    }

    static uint8_t recv_byte() {
        uint8_t val = 0;
        SDA::set();
        for (uint8_t i = 0; i < 8; i ++) {
            SCL::set();
            delay();
            val <<= 1;
            if (SDA::read()) {
                val ++;
            }
            delay();
            SCL::clr();
            delay();
        }
        return val;
    }

    static void write_ack() {
        SDA::clr();
        delay();
        SCL::set();
        delay();
        SCL::clr();
        delay();
    }

    static void write_no_ack() {
        SDA::set();
        delay();
        SCL::set();
        delay();
        SCL::clr();
        delay();
    }

    static bool wait_ack() {
        bool flag = false;
        SDA::set();
        SCL::set();
        delay();
        flag = !SDA::read();
        delay();
        SCL::clr();
        delay();
        return flag;
    }

private:
    static void delay() {
        for (volatile uint32_t i = 0; i < 1000; i ++);
    }
};

}

#endif
